/* Copyright (c) 2022-2023 Qualcomm Technologies, Inc.
 * All Rights Reserved.
 * Confidential and Proprietary - Qualcomm Technologies, Inc.
 */

#include <stdio.h>
#include <sched.h>
#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <thread>
#include <pthread.h>
#include <rclcpp/rclcpp.hpp>

#include "tracker.h"
#include<sys/time.h>

bool running = true;
static void INT_handler (int sig)
{
    std::cout << "signal received to stop" << std::endl;
    running = false;
}

int main(int argc, char** argv)
{
    std::string model_param_file = std::string( "/data/model/FastestDet.param" );
    std::string model_bin_file = std::string( "/data/model/FastestDet.bin" );
    std::string label_file = std::string( "/data/model/labels.txt" );

    if (argc == 4)
    {
        model_param_file = std::string(argv[1]);
        model_bin_file = std::string(argv[2]);
        label_file = std::string(argv[3]);
    }

    rclcpp::init(argc, argv);
    auto g_node = rclcpp::Node::make_shared("eye_node");



    signal(SIGINT, INT_handler);

    int tracking_type = TRACKING_TYPE_CV;

    /* run this on the super core */
    cpu_set_t mask;
    CPU_ZERO(&mask);
    CPU_SET(6, &mask);

    sched_setaffinity(0, sizeof(mask), &mask); //fisrt param 0 means this PID

    double rate = 30;
    float theta, d;

    MyTracker tracker(tracking_type, model_param_file, model_bin_file, label_file);
    bool result;

    rclcpp::Rate loop_rate(rate);
    tracker.target_info_pub = g_node->create_publisher<geometry_msgs::msg::Vector3>("/cmd_vel", 10);
    // tracker.kcfTrackingInit(running);
    tracker.getImageOnce();

    std::thread getImage_thread(&MyTracker::getImage_thread, &tracker, std::ref(running));

    struct  timeval  tv_begin, tv_end;
    double totalTimeMain = 0, cnt = 0;
    while(rclcpp::ok() && running)
    {
        std::vector<TargetBox> target_boxes;
        gettimeofday(&tv_begin, NULL);
        tracker.camLock();
        result = tracker.trackingOnce(target_boxes);
        if(result)
        {
            tracker.publishTargetInfo(target_boxes); 
        }

        rclcpp::spin_some(g_node);
        loop_rate.sleep();

        gettimeofday(&tv_end, NULL);
        int time =(tv_end.tv_sec - tv_begin.tv_sec) * 1000 * 1000 + (tv_end.tv_usec - tv_begin.tv_usec);
        totalTimeMain += time;
        std::cout << "using time: " << time / 1000.f << "ms" << std::endl;
    }

    getImage_thread.join();
    tracker.stopTracking();

    sleep(1);  //to make sure stop cmd received

    RCLCPP_INFO(g_node->get_logger(), "%s\n", "follow-me stopped");
    fflush(stdout);
    rclcpp::shutdown();

    return 0;
}
